#define BLINKER_BLE
#include <Blinker.h>
#define JOY_1 "JOYKey"

BlinkerJoystick JOY1(JOY_1);

// pwm通道 PWM一共有16个通道，0-7位高速通道由80Mhz时钟驱动，后面8个为低速通道由1Mhz时钟驱动
int FRONT_LEFT_EN = 2;
int FRONT_LEFT_IN1 = 4;
int FRONT_LEFT_IN2 = 16;
int FRONT_RIGHT_EN = 18;
int FRONT_RIGHT_IN3 = 17;
int FRONT_RIGHT_IN4 = 5;
int REAR_LEFT_EN = 12;
int REAR_LEFT_IN1 = 14;
int REAR_LEFT_IN2 = 27;
int REAR_RIGHT_EN = 33;
int REAR_RIGHT_IN3 = 26;
int REAR_RIGHT_IN4 = 25;


void joystick1_callback(uint8_t joystickX, uint8_t joystickY)
{
  // 根据摇杆值计算电机速度
  int frontLeftSpeed = joystickY + joystickX;
  int frontRightSpeed = joystickY - joystickX;
  int rearLeftSpeed = joystickY + joystickX;
  int rearRightSpeed = joystickY - joystickX;

  // 限制速度范围
  frontLeftSpeed = constrain(frontLeftSpeed, 0, 255);
  frontRightSpeed = constrain(frontRightSpeed, 0, 255);
  rearLeftSpeed = constrain(rearLeftSpeed, 0, 255);
  rearRightSpeed = constrain(rearRightSpeed, 0, 255);

  // 控制电机
  if (frontLeftSpeed > 127) {
    digitalWrite(FRONT_LEFT_IN1, HIGH);
    digitalWrite(FRONT_LEFT_IN2, LOW);
    analogWrite(FRONT_LEFT_EN, frontLeftSpeed - 127);
  } else {
    digitalWrite(FRONT_LEFT_IN1, LOW);
    digitalWrite(FRONT_LEFT_IN2, HIGH);
    analogWrite(FRONT_LEFT_EN, 127 - frontLeftSpeed);
  }

  if (frontRightSpeed > 127) {
    digitalWrite(FRONT_RIGHT_IN3, LOW);
    digitalWrite(FRONT_RIGHT_IN4, HIGH);
    analogWrite(FRONT_RIGHT_EN, frontRightSpeed - 127);
  } else {
    digitalWrite(FRONT_RIGHT_IN3, HIGH);
    digitalWrite(FRONT_RIGHT_IN4, LOW);
    analogWrite(FRONT_RIGHT_EN, 127 - frontRightSpeed);
  }

  if (rearLeftSpeed > 127) {
    digitalWrite(REAR_LEFT_IN1, HIGH);
    digitalWrite(REAR_LEFT_IN2, LOW);
    analogWrite(REAR_LEFT_EN, rearLeftSpeed - 127);
  } else {
    digitalWrite(REAR_LEFT_IN1, LOW);
    digitalWrite(REAR_LEFT_IN2, HIGH);
    analogWrite(REAR_LEFT_EN, 127 - rearLeftSpeed);
  }

  if (rearRightSpeed > 127) {
    digitalWrite(REAR_RIGHT_IN3, LOW);
    digitalWrite(REAR_RIGHT_IN4, HIGH);
    analogWrite(REAR_RIGHT_EN, rearRightSpeed - 127);
  } else {
    digitalWrite(REAR_RIGHT_IN3, HIGH);
    digitalWrite(REAR_RIGHT_IN4, LOW);
    analogWrite(REAR_RIGHT_EN, 127 - rearRightSpeed);
  }
}

void setup() 
{
  pinMode(FRONT_LEFT_IN1, OUTPUT);
  pinMode(FRONT_LEFT_IN2, OUTPUT);
  pinMode(FRONT_LEFT_EN, OUTPUT);
  pinMode(FRONT_RIGHT_IN3, OUTPUT);
  pinMode(FRONT_RIGHT_IN4, OUTPUT);
  pinMode(FRONT_RIGHT_EN, OUTPUT);
  pinMode(REAR_LEFT_IN1, OUTPUT);
  pinMode(REAR_LEFT_IN2, OUTPUT);
  pinMode(REAR_LEFT_EN, OUTPUT);
  pinMode(REAR_RIGHT_IN3, OUTPUT);
  pinMode(REAR_RIGHT_IN4, OUTPUT);
  pinMode(REAR_RIGHT_EN, OUTPUT);

  
  Blinker.begin();  
  JOY1.attach(joystick1_callback);
}

void loop() 
{
  Blinker.run();
}